

#ifndef __COMM_MSG_NAV_INFO_H__
#define __COMM_MSG_NAV_INFO_H__

#include "comm/comm_topic_name.h"
#include "comm/comm_base.h"
#include "comm/comm_msg.h"
#include <comm_msg/nav_info.h>
#include <ros/ros.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

typedef struct 
{
    UPSERIAL_WORK_ROBOT_STATUS_ENUM enRobotState;
    UPSERIAL_WORK_NAV_STATUS_ENUM   enNavState;
    float                           completeRate;
    float                           leftRouteLen;
} NAV_INFO_STRU;

/**************************************************************************************
功能描述: 发布消息
修改记录:
**************************************************************************************/
class NAV_INFO_SENDER_C
{
public:
    NAV_INFO_SENDER_C(ros::NodeHandle &nh)
    {
        publisher_ = nh.advertise<comm_msg::nav_info>(TOPIC_NAME_NAV_INFO, 1);
    }

    void Send(const NAV_INFO_STRU &stNavInfo)
    {
        comm_msg::nav_info stMsg;

        stMsg.robotState    = (UINT08)stNavInfo.enRobotState;
        stMsg.navState      = (UINT08)stNavInfo.enNavState;
        stMsg.completeRate  = stNavInfo.completeRate;
        stMsg.leftRouteLen  = stNavInfo.leftRouteLen;

        publisher_.publish(stMsg);
    }

private:
    ros::Publisher publisher_;
};

/**************************************************************************************
功能描述: 接收消息
修改记录:
**************************************************************************************/
class NAV_INFO_RECEIVER_C
{
public:
    NAV_INFO_RECEIVER_C(ros::NodeHandle &nh, double overTime = 0.15)
    {
        subscriber_ = nh.subscribe(TOPIC_NAME_NAV_INFO, 1, &NAV_INFO_RECEIVER_C::ReceiveMsgCallBack, this);

        objInvalidTimer_.SetOverTime(overTime);
        stNavInfo_.enRobotState = UPSERIAL_WORK_ROBOT_STATUS_IDLE;
        stNavInfo_.enNavState   = UPSERIAL_WORK_NAV_STATUS_IDEL;
        stNavInfo_.completeRate = 1.0;
        stNavInfo_.leftRouteLen = 0.0;
    }

    INT32 Get(NAV_INFO_STRU &stNavInfo) const
    {
        if (objInvalidTimer_.IsTimeOut()) {
            return -1;
        }
        
        stNavInfo = stNavInfo_;
        return 0;
    }
    
private:
    ros::Subscriber subscriber_;

    TIMER_C         objInvalidTimer_;
    NAV_INFO_STRU   stNavInfo_;

    void ReceiveMsgCallBack(const comm_msg::nav_info::ConstPtr &pstMsg)
    {
        R_ASSERT(pstMsg != NULL, NOTHING);

        objInvalidTimer_.Reset();

        stNavInfo_.enRobotState = (UPSERIAL_WORK_ROBOT_STATUS_ENUM)pstMsg->robotState;
        stNavInfo_.enNavState   = (UPSERIAL_WORK_NAV_STATUS_ENUM)pstMsg->navState;
        stNavInfo_.completeRate = pstMsg->completeRate;
        stNavInfo_.leftRouteLen = pstMsg->leftRouteLen;
    }
};


#endif

